/** @internal
 ** @file    aibpush.c
 ** @brief   Push data down the AIB tree - MEX driver
 ** @author  Andrea Vedaldi
 **/

/* AUTORIGHTS
Copyright 2007 (c) Andrea Vedaldi and Brian Fulkerson

This file is part of VLFeat, available in the terms of the GNU
General Public License version 2.
*/

#include<mexutils.h>

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include<string.h>
#include<assert.h>

#include <vl/generic.h>

/* ---------------------------------------------------------------- */
/** @brief MEX driver entry point 
 **/

void 
mexFunction (int nout, mxArray * out[], int nin, const mxArray * in[])
{
  enum {IN_PARENTS = 0, IN_DATA} ;
  enum {OUT_TREE} ;

  vl_uint32 const *parents ;  
  vl_uint32 *tree ;
  double const *data ;

  int nnull = 0 ;
  
  int i, P, N ;

  /* -----------------------------------------------------------------
   *                                               Check the arguments
   * -------------------------------------------------------------- */

  if (nin != 2) {
    mexErrMsgTxt ("Two arguments required.") ;
  }

  if (nout > 1) {
    mexErrMsgTxt ("Too many output arguments.") ;
  }
  
  if (!uIsRealMatrix(in[IN_DATA], -1, -1)) {
    mexErrMsgTxt ("DATA must be a matrix of DOUBLE");
  }

  if (!uIsVector(in[IN_PARENTS], -1)) {
    mexErrMsgTxt ("PARENTS must be a vector") ;   
  }

  if (mxGetClassID(in[IN_PARENTS]) != mxUINT32_CLASS) {
    mexErrMsgTxt ("PARENTS must be UINT32") ;
  }
  
  N = mxGetNumberOfElements (in[IN_DATA]) ;
  data = mxGetPr (in[IN_DATA]) ;

  P = mxGetNumberOfElements (in[IN_PARENTS]) ;
  parents = mxGetData (in[IN_PARENTS]) ;
  
  out[OUT_TREE] = mxCreateNumericMatrix(1, P,mxUINT32_CLASS, mxREAL) ;
  tree = mxGetData (out[OUT_TREE]) ;

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */

  {   
    char buf [1024] ;
    vl_uint32 max_node = 0 ;
    vl_uint32 min_node = 0 ;
    vl_uint32 last_leaf = 0 ;
    vl_uint32 root = 0 ;

    /* exhamine parents for errors and informations */
    for (i = 0 ; i  < P ; ++i) {
      vl_uint32 node = parents [i] ;

      if ((node != 0) & (node != 1)) {
        max_node = VL_MAX (node, max_node) ;
        min_node = VL_MIN (node, min_node) ;
      }

      /* check no node points outside the tree */
      if (node > P) {
        snprintf(buf, sizeof(buf),
                 "Out of bounds link PARENTS[%d] = %u > %u", i, node, P) ;
        mexErrMsgTxt (buf) ;
      }
      
      /* check node points to something above him */
      if ((node != 0) & (node != 1) & (node < i)) {
        snprintf(buf, sizeof(buf),
                 "Backward link PARENTS[%d] = %u < %d", i, node, i) ;
        mexErrMsgTxt (buf) ;
      }
      if (node == 0) ++ nnull ;      
    }

    /* now
     *
     * min_node = first node which is not a leaf
     * max_node = root node
     * nnull    = number of leaves pointing to the null node
     */
    
    last_leaf = min_node - 1 ;
    root = max_node ;
    
    /* process data */
    for (i = 0 ; i < N ; ++i) {
      int x = (int) data [i] ;
      
      if ((x < 1) | (x > last_leaf)) {
        snprintf(buf, sizeof(buf),
                 "DATA [%d] = %d is not a leaf", i, x) ;
        mexErrMsgTxt (buf) ;        
      }
      
      while (true) {
        int x_ = (int) parents [x -1] ;
        /*     mexPrintf("%d : x_=%d, x=%d\n", i, x_, x) ; */
        ++ tree [x - 1] ;        
        if ((x_ == x) | (x_ == 0) | (x_ == 1)) break ;
        x = x_ ;
      }
    }
  }
}
